use cmsis_rtos2_rs::os_delay;
use periph_utils::GpioConfig;
use periph_utils::stm32f1;
use utils::defer_ref;

use crate::cp_drop::DropErr;

use super::sensor;


#[derive(GpioConfig)]
#[gpio(init = "gpio_init",
    io(name = "drop1", io = "PB0", mode = "OUT_PP", set, reset),
    io(name = "drop2", io = "PB9", mode = "OUT_PP", set, reset),
    io(name = "drop3", io = "PB10", mode = "OUT_PP", set, reset),
    io(name = "light_led", io = "PB11", mode = "OUT_PP", set, reset),
    io(name = "fan", io = "PB12", mode = "OUT_PP", set, reset),
    io(name = "film_power", io = "PB13", mode = "OUT_PP", set, reset),
    io(name = "robot_power", io = "PB14", mode = "OUT_PP", set, reset),
    io(name = "drain_pump", io = "PB15", mode = "OUT_PP", set, reset),
)]
struct Pin;

const DROP_CUP_TIMEOUT: u32 = 10 * 1000;

pub unsafe fn init() {
    Pin::gpio_init();
}

#[inline(always)]
pub fn drop1_ctrl() -> Result<(), DropErr> {
    drop_ctrl::<D1>()
}

#[inline(always)]
pub fn drop2_ctrl() -> Result<(), DropErr> {
    drop_ctrl::<D2>()
}

#[inline(always)]
pub fn drop3_ctrl() -> Result<(), DropErr> {
    drop_ctrl::<D3>()
}

pub fn light_led_open() {
    Pin::light_led_reset();
}

pub fn light_led_close() {
    Pin::light_led_set();
}

pub fn fan_open() {
    Pin::fan_set();
}

pub fn fan_close() {
    Pin::fan_reset();
}

pub fn film_power_restart() {
    Pin::film_power_set();
    os_delay(15000);
    Pin::film_power_reset();
}

pub fn robot_power_restart() {
    Pin::robot_power_set();
    os_delay(15000);
    Pin::robot_power_reset();
}

pub fn drain_pump_open() {
    Pin::drain_pump_set();
}

pub fn drain_pump_close() {
    Pin::drain_pump_reset();
}

trait CupCtrl {
    fn on();
    fn off();
    fn is_checked() -> bool;
    fn is_no_empty() -> bool;
    const TIME_OUT_MS: u32 = DROP_CUP_TIMEOUT;
    const TIME_OUT_ERR: DropErr;
}

struct D1;
struct D2;
struct D3;

impl CupCtrl for D1 {
    fn is_checked() -> bool {
        sensor::is_drop_sw1()
    }
    fn is_no_empty() -> bool {
        sensor::is_drop_checked1()
    }
    fn on() {
        Pin::drop1_set();
    }
    fn off() {
        Pin::drop1_reset();
    }
    const TIME_OUT_ERR: DropErr = DropErr::Drop1Timeout;
}

impl CupCtrl for D2 {
    fn is_checked() -> bool {
        sensor::is_drop_sw2()
    }
    fn is_no_empty() -> bool {
        sensor::is_drop_checked2()
    }
    fn on() {
        Pin::drop2_set();
    }
    fn off() {
        Pin::drop2_reset();
    }
    const TIME_OUT_ERR: DropErr = DropErr::Drop2Timeout;
}

impl CupCtrl for D3 {
    fn is_checked() -> bool {
        sensor::is_drop_sw3()
    }
    fn is_no_empty() -> bool {
        sensor::is_drop_checked3()
    }
    fn on() {
        Pin::drop3_set();
    }
    fn off() {
        Pin::drop3_reset();
    }
    const TIME_OUT_ERR: DropErr = DropErr::Drop3Timeout;
}

fn drop_ctrl<T: CupCtrl>() -> Result<(), DropErr> {

    let mut time_count = 0;
    
    if !T::is_no_empty() {
        return Err(DropErr::DropEmpty);
    }
    T::on();
    defer_ref!({
        T::off();
    });

    while T::is_checked() {
        os_delay(20);
        time_count += 20;
        if time_count >= T::TIME_OUT_MS {
            return Err(T::TIME_OUT_ERR);
        }
    }

    time_count = 0;
    while !T::is_checked() {
        os_delay(20);
        time_count += 20;
        if time_count >= T::TIME_OUT_MS {
            return Err(T::TIME_OUT_ERR);
        }
    }

    Ok(())
}
